/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package com.nurdrobotics;

import edu.wpi.first.wpilibj.*;

/**
 *
 */
public class Shooter {

    private static final double pitchMaxSpeedRatio = 0.5;
    private static SpeedController m_jaguarLeft1 = null;
    private static SpeedController m_jaguarLeft2 = null;
    private static SpeedController m_jaguarRight1 = null;
    private static SpeedController m_jaguarRight2 = null;
    private static SpeedController m_jaguarPitch = null;
    private static LimitSwitch pitchUp = null;
    private static LimitSwitch pitchDown = null;
    private static LimitSwitch twoPointLimit = null;

    /*
     * This function initializes variables in this class. It should
     * be called exactly once prior to using the class.
     */
    public static boolean init() {
        m_jaguarLeft1 = Motor.getInstance(Motor.ID_SHOOTER_LEFT1);
        m_jaguarLeft2 = Motor.getInstance(Motor.ID_SHOOTER_LEFT2);
        m_jaguarRight1 = Motor.getInstance(Motor.ID_SHOOTER_RIGHT1);
        m_jaguarRight2 = Motor.getInstance(Motor.ID_SHOOTER_RIGHT2);
        m_jaguarPitch = Motor.getInstance(Motor.ID_SHOOTER_PITCH);
        if ((m_jaguarLeft1 == null) || (m_jaguarRight1 == null)
                || (m_jaguarLeft2 == null) || (m_jaguarRight2 == null)
                || (m_jaguarPitch == null)) {
            Dashboard.setShooterStatus("MER");
            return false;
        }

        pitchUp = new LimitSwitch(DigitalSidecar.INPUT_PITCH_UP_SWITCH);
        pitchDown = new LimitSwitch(DigitalSidecar.INPUT_PITCH_DOWN_SWITCH);
        twoPointLimit = new LimitSwitch(DigitalSidecar.INPUT_TWO_POINT_SWITCH);

        if ((pitchUp == null) || (pitchDown == null) || (twoPointLimit == null)) {
            Dashboard.setShooterStatus("SER");
            return false;
        }

        return true;

    }

    /**
     * This function is called continuously while in tele-operated mode
     */
    public static void teleopUpdate() {
        double speed = 0;

        if (ShooterStick.disableShooter()) {

             //m_jaguarLeft1.set(0);
             //m_jaguarLeft2.set(0);
             //m_jaguarRight1.set(0);
             //m_jaguarRight2.set(0);
            set(0);
            
            //update shooter speed on dashboard
            Dashboard.setShooterSpeedStatus("0");

        } else {
            speed = ShooterStick.getMotorSpeed();

            m_jaguarLeft1.set(speed);
            m_jaguarLeft2.set(speed);

            // run the right motors in reverse to the left motors
            m_jaguarRight1.set(-speed);
            m_jaguarRight2.set(-speed);

            //update shooter speed on dashboard
            int iSpeed = (int) Math.abs(speed * 100);
            Dashboard.setShooterSpeedStatus("" + iSpeed);

        }
        /*System.out.println("jaguarLeft1 speed = " + m_jaguarLeft1.get());
         *System.out.println("jaguarLeft2 speed = " + m_jaguarLeft2.get());
         *System.out.println("jaguarRight1 speed = " + m_jaguarRight1.get());
         *System.out.println("jaguarRight2 speed = " + m_jaguarRight2.get());
         */

        double pitchValue = ShooterStick.getPitchValue();

        boolean autoExtend = twoPointLimit.isOpen() && ShooterStick.autoExtendPressed();
        boolean autoRetract = pitchDown.isOpen() && ShooterStick.autoRetractPressed();
        boolean okToPitchUp = pitchUp.isOpen();
        boolean okToPitchDown = pitchDown.isOpen();
        boolean okToMove = okToPitchUp || okToPitchDown;

       // System.out.println("pitchValue="+ pitchValue + " okToPitchDown=" + okToPitchDown + " okToPitchUp=" + okToPitchUp);

        if (autoExtend) {
            speed = (1 * pitchMaxSpeedRatio);
        } else if (autoRetract) {
            speed = (-1 * pitchMaxSpeedRatio);
        } else if ((pitchValue < 0) && (okToPitchUp)) {
            // assume motor drives inverted to stick
            // fwd on stick = 1, which results in -1 to pitch down (negative motor)
            speed = (-pitchValue * pitchMaxSpeedRatio);
        } else if ((pitchValue > 0) && (okToPitchDown)) {
            // assume motor drives inverted to stick
            // fwd on stick = 1, which results in -1 to pitch down (negative motor)
            speed = (-pitchValue * pitchMaxSpeedRatio);
        } else {
           // System.out.println("ZERO");

            speed = (0);
        }
        
        m_jaguarPitch.set(speed);
    }

    //creates a method to reverse/eject the shooter
    public static void reverse() {
        double speed = -ShooterStick.getMotorSpeed();
        set(speed);
    }

    //creates a method to set all four motor speeds given one double value
    //private method; called only in the Shooter class
    private static void set (double speed) {
        m_jaguarLeft1.set(speed);
        m_jaguarLeft2.set(speed);

        // run the right motors in reverse to the left motors
        m_jaguarRight1.set(-speed);
        m_jaguarRight2.set(-speed);

    }
}